/**
 * @file:          Filter_Notch.c
 * @brief:
 * @details:
 * @author:        wjh
 * @date created:  2023.07.28
 * @version:       1.0.0
 * @par copyright (c):
 *
 * @par history (desc):
 *   version:1.0.0, wjh, 2023.07.28,16:51:34
 */

/* Include Files **************************************************************/
#include "Filter_Notch.h"
#include "common.h"

/* Global Variable Define *****************************************************/

/* Function Define ************************************************************/

void Filter_Notch_Init(struct Filter_Notch *self)
{
    self->Run = Filter_Notch_Run;
    self->Clear = Filter_Notch_Clear;
    self->UpdateParam = Filter_Notch_UpdateParam;

    self->UpdateParam(self);
}

float32 Filter_Notch_Run(struct Filter_Notch *self, float32 in)
{
    if (self->param_en != 0)
    {
        self->y_last2 = self->y_last1;
        self->y_last1 = self->out_y;
        self->x_last2 = self->x_last1;
        self->x_last1 = self->in_x;
        self->in_x = in;

        self->out_y = self->m[0] * self->in_x +
                      self->m[1] * self->x_last1 +
                      self->m[2] * self->x_last2 +
                      self->n[0] * self->y_last1 +
                      self->n[1] * self->y_last2;
    }
    else
    {
        self->in_x = in;
        self->out_y = self->in_x;
    }

    return self->out_y;
}

void Filter_Notch_Clear(struct Filter_Notch *self)
{
    self->in_x = 0;
    self->y_last1 = 0;
    self->x_last1 = 0;
    self->y_last2 = 0;
    self->x_last2 = 0;
    self->out_y = 0;
}

void Filter_Notch_UpdateParam(struct Filter_Notch *self)
{
    self->w0 = BM_2PI * self->param_f0;
    self->w_width = BM_2PI * self->param_f_width;

    float32 a;
    float32 b;
    a = 4 * squre(BM_PI * self->param_Ts * self->param_f0);
    b = 6.0 * BM_PI * self->param_Ts * self->param_f_width;
    self->m[0] = (a + b * self->param_depth + 4) / (a + b + 4);
    self->m[1] = (2 * a - 8) / (a + b + 4);
    self->m[2] = (a - b * self->param_depth + 4) / (a + b + 4);
    self->n[0] = (-2 * a + 8) / (a + b + 4);
    self->n[1] = (-a + b - 4) / (a + b + 4);
}